//pid控制 能在线调参
#include <rclcpp/rclcpp.hpp>

// MoveIt Servo
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <control_msgs/msg/joint_jog.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/wrench_stamped.hpp>
#include <tf2_ros/buffer.h>

#include <rcl_interfaces/msg/set_parameters_result.hpp>  // rqt/CLI 动态参数
#include <atomic>
#include <algorithm>
#include <cmath>

using namespace std::chrono_literals;

static const rclcpp::Logger LOGGER =
    rclcpp::get_logger("smc0.servo_node.cpp");

// 节点/通信句柄
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr sim_force_sub_;
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr g_param_cb_handle;

// ===========================
// 恒力控制相关
// ===========================
constexpr double kFzTarget = 5.0;  // 目标恒力 5N（Z轴）
constexpr double kMaxI = 2.0;      // 积分限幅

// 可在线调的 PID 与速度上限（rqt/ros2 param set）
std::atomic<double> g_kp{0.05};
std::atomic<double> g_ki{0.00};
std::atomic<double> g_kd{0.00};
std::atomic<double> g_max_vel{0.5};

// 状态量
std::atomic<double> g_latest_fz{0.0};  // 最近一次传感器 Z 向力
double g_int_acc = 0.0;                // 积分项
double g_prev_err = 0.0;               // 上次误差
rclcpp::Time g_prev_time;              // 上次控制时刻
bool g_has_prev_time = false;

// ============
// /sim_force 回调：更新Fz
// ============
void simForceCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg)
{
  g_latest_fz.store(msg->wrench.force.z, std::memory_order_relaxed);

  // 节流打印（单位：毫秒，Humble 需整数）
  RCLCPP_INFO_THROTTLE(LOGGER, *node_->get_clock(), 500,
                       "[/sim_force] Fz=%.3f N (target=%.2f)",
                       msg->wrench.force.z, kFzTarget);
}

// ============
// 定时器：PID 计算并发布 Twist
// ============
void publishCommands()
{
  // 计算 dt
  const rclcpp::Time now = node_->now();
  double dt = 0.05;  // 默认 50ms
  if (g_has_prev_time) {
    dt = std::max(1e-3, (now - g_prev_time).seconds());  // 下限 1ms 防止除零
  }
  g_prev_time = now;
  g_has_prev_time = true;

  // 读取当前力与误差
  const double fz = g_latest_fz.load(std::memory_order_relaxed);
  const double err = kFzTarget - fz;  // <5N -> 正误差 -> +x；>5N -> 负误差 -> -x

  // 读取当前增益/限速（原子读）
  const double Kp = g_kp.load(std::memory_order_relaxed);
  const double Ki = g_ki.load(std::memory_order_relaxed);
  const double Kd = g_kd.load(std::memory_order_relaxed);
  const double Vmax = g_max_vel.load(std::memory_order_relaxed);

  // PID
  g_int_acc += err * dt;
  g_int_acc = std::clamp(g_int_acc, -kMaxI, kMaxI);

  const double derr = (err - g_prev_err) / dt;
  g_prev_err = err;

  double ux = Kp * err + Ki * g_int_acc + Kd * derr;

  // 速度限幅
  if (ux >  Vmax) ux =  Vmax;
  if (ux < -Vmax) ux = -Vmax;

  // 组包发布
  auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
  msg->header.stamp = now;
  msg->header.frame_id = "base_link";

  msg->twist.linear.x = ux;  // X向速度由PID输出
  msg->twist.linear.y = 0.0;
  msg->twist.linear.z = 0.0;
  msg->twist.angular.x = 0.0;
  msg->twist.angular.y = 0.0;
  msg->twist.angular.z = 0.0;

  twist_cmd_pub_->publish(std::move(msg));

  // 节流打印 PID 关键信息
  RCLCPP_INFO_THROTTLE(LOGGER, *node_->get_clock(), 500,
                       "[PID] Fz=%.3fN err=%.3f u_x=%.3fm/s (dt=%.3fs, I=%.3f)  [Kp=%.4f Ki=%.4f Kd=%.4f Vmax=%.3f]",
                       fz, err, ux, dt, g_int_acc, Kp, Ki, Kd, Vmax);
}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;

  // 目前先设为 false，直到 MoveIt 修正 QoS 设置以启用进程内通信
  node_options.use_intra_process_comms(false);
  node_ = std::make_shared<rclcpp::Node>("servo_node", node_options);

  // 等待 RViz/环境就绪
  rclcpp::sleep_for(std::chrono::seconds(4));

  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
  auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
      node_, "robot_description", tf_buffer, "planning_scene_monitor");

  // 确保 PSM 能从 /joint_states 实时更新机器人状态
  if (planning_scene_monitor->getPlanningScene())
  {
    planning_scene_monitor->startStateMonitor("/joint_states");
    planning_scene_monitor->setPlanningScenePublishingFrequency(25);
    planning_scene_monitor->startPublishingPlanningScene(
        planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
        "/moveit_servo/publish_planning_scene");
    planning_scene_monitor->startSceneMonitor();
    planning_scene_monitor->providePlanningSceneService();
  }
  else
  {
    RCLCPP_ERROR(LOGGER, "Planning scene 未正确配置");
    return EXIT_FAILURE;
  }

  // Publisher/Subscriber
  twist_cmd_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>(
      "servo_node/delta_twist_cmds", 10);

  sim_force_sub_ = node_->create_subscription<geometry_msgs::msg::WrenchStamped>(
      "/sim_force",
      rclcpp::SensorDataQoS(),
      &simForceCallback);

  // MoveIt Servo
  auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_);
  auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_parameters, planning_scene_monitor);
  servo->start();

  // —— 可在线调参数：声明 + 初始同步 ——
  node_->declare_parameter<double>("kp", g_kp.load());
  node_->declare_parameter<double>("ki", g_ki.load());
  node_->declare_parameter<double>("kd", g_kd.load());
  node_->declare_parameter<double>("max_vel", g_max_vel.load());

  g_kp.store(node_->get_parameter("kp").as_double());
  g_ki.store(node_->get_parameter("ki").as_double());
  g_kd.store(node_->get_parameter("kd").as_double());
  g_max_vel.store(node_->get_parameter("max_vel").as_double());

  // 参数在线更新回调
  g_param_cb_handle = node_->add_on_set_parameters_callback(
    [](const std::vector<rclcpp::Parameter>& params)
    {
      rcl_interfaces::msg::SetParametersResult result;
      result.successful = true;
      for (const auto& p : params)
      {
        if (p.get_name() == "kp") {
          g_kp.store(p.as_double());
          RCLCPP_INFO(LOGGER, "kp -> %.6f", g_kp.load());
        } else if (p.get_name() == "ki") {
          g_ki.store(p.as_double());
          RCLCPP_INFO(LOGGER, "ki -> %.6f", g_ki.load());
        } else if (p.get_name() == "kd") {
          g_kd.store(p.as_double());
          RCLCPP_INFO(LOGGER, "kd -> %.6f", g_kd.load());
        } else if (p.get_name() == "max_vel") {
          g_max_vel.store(p.as_double());
          RCLCPP_INFO(LOGGER, "max_vel -> %.6f", g_max_vel.load());
        }
      }
      return result;
    }
  );

  // 控制循环（50ms=20Hz；需要更快可改为 10ms=100Hz）
  rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);

  auto executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
  executor->add_node(node_);
  executor->spin();

  rclcpp::shutdown();
  return 0;
}
